﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace uam_fps_game.Physics
{
    public class RigidBody
    {
        public Vector3 Position
        {
            get { return _position; }
            set { _position = value; }
        }

        public Quaternion Orientation
        {
            get { return _orientation; }
            set { _orientation = value; }
        }

        public Matrix WorldMatrix
        {
            get { return _worldMatrix; }
            set { _worldMatrix = value; }
        }

        protected float _mass;
        protected float _massInverse;

        protected Matrix _inertiaTensor;
        protected Matrix _inertiaTensorInverse;

        protected Vector3 _position;
        protected Quaternion _orientation;

        protected Matrix _worldMatrix;

        protected Vector3 _velocity;
        protected Vector3 _angularVelocity;

        protected Vector3 _forceAccum;
        protected Vector3 _torqueAccum;

        private static float LINEAR_DAMPING = 0.95f;
        private static float ANGULAR_DAMPING = 0.75f;

        internal RigidBody()
        {

        }

        public void Init(Model model, Vector3 position, Vector3 orientation)
        {
            RigidBodyProperties props = new RigidBodyProperties();
            RigidBodyProperties.CalculateFromConvexTriangleMesh(model, out props);

            Init(props.Mass, props.InertiaTensor, position, orientation);
        }

        public void Init(float mass, Matrix inertiaTensor, Vector3 position, Vector3 orientation)
        {
            _mass = mass;
            _massInverse = 1.0f / mass;

            _inertiaTensor = inertiaTensor;
            _inertiaTensorInverse = Matrix.Invert(inertiaTensor);

            _position = position;
            _orientation = Quaternion.CreateFromYawPitchRoll(orientation.Y, orientation.X, orientation.Z);

            UpdateWorldMatrix();

            _velocity = Vector3.Zero;
            _angularVelocity = Vector3.Zero;
            ClearAccums();
        }

        public void AddForce(ref Vector3 force)
        {
            Vector3.Add(ref _forceAccum, ref force, out _forceAccum);
        }

        public void AddForceAtPoint(ref Vector3 force, ref Vector3 point) // point in world coordinates
        {
            Vector3.Add(ref _forceAccum, ref force, out _forceAccum);

            Vector3 torque;
            Vector3 pointRelative;
            Vector3.Subtract(ref point, ref _position, out pointRelative);
            Vector3.Cross(ref pointRelative, ref force, out torque);
            Vector3.Add(ref _torqueAccum, ref torque, out _torqueAccum);
        }

        internal void ClearAccums()
        {
            _forceAccum = Vector3.Zero;
            _torqueAccum = Vector3.Zero;
        }

        void UpdateWorldMatrix()
        {
            _worldMatrix = Matrix.CreateFromQuaternion(_orientation);
            _worldMatrix.Translation = _position;
        }

        void UpdateOrientation(float dt)
        {
            float tmp = dt * 0.5f;
            Quaternion orientationChange = new Quaternion(_angularVelocity.X * tmp, _angularVelocity.Y * tmp, _angularVelocity.Z * tmp, 0.0f);
            Quaternion.Multiply(ref orientationChange, ref _orientation, out orientationChange);
            Quaternion.Add(ref _orientation, ref orientationChange, out _orientation);

            _orientation.Normalize();
        }

        internal void Update(float dt)
        {
            // linear motion
            Vector3 acc;
            Vector3.Multiply(ref _forceAccum, _massInverse * dt, out acc);
            Vector3.Add(ref _velocity, ref acc, out _velocity);

            Vector3.Multiply(ref _velocity, (float)Math.Pow(LINEAR_DAMPING, dt), out _velocity);

            Vector3 move;
            Vector3.Multiply(ref _velocity, dt, out move);
            Vector3.Add(ref _position, ref move, out _position);

            
            // rotational motion
            Matrix worldRot = _worldMatrix;
            worldRot.Translation = Vector3.Zero;
            Matrix worldRotTranspose;
            Matrix.Transpose(ref worldRot, out worldRotTranspose);

            Matrix wsInertiaTensorInverse; // world-space inertia tensor inverse
            Matrix.Multiply(ref worldRot, ref _inertiaTensorInverse, out wsInertiaTensorInverse);
            Matrix.Multiply(ref wsInertiaTensorInverse, ref worldRotTranspose, out wsInertiaTensorInverse);

            Vector3 angAcc;
            Vector3.Transform(ref _torqueAccum, ref wsInertiaTensorInverse, out angAcc);
            Vector3.Multiply(ref angAcc, dt, out angAcc);
            Vector3.Add(ref _angularVelocity, ref angAcc, out _angularVelocity);

            Vector3.Multiply(ref _angularVelocity, (float)Math.Pow(ANGULAR_DAMPING, dt), out _angularVelocity);

            UpdateOrientation(dt);

            //
            UpdateWorldMatrix();
            ClearAccums();
        }
    }
}
